/*  
 *  Nanjing JIYU Technology Co.,Ltd All rights reserved.
 *  Copyright (C) 2022 
 *  ==============================================================
 * 
 *  automatic.c 
 *    自动运行模式下运行逻辑的实现
 *
 *  --------------------------------------------------------------
 *  
 */

#include "automatic.h"

#include "status.h"
#include "track.h"

#include "device.h"


static void auto_0_1(void)
{
  /* 小车运行 */
  while (1)
  {
    delay_ms(20);
    track_drive(speed_medium);

    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_cross())
    {
      track_stop();
      break;
    }
  }

  status.location = '1';    /* 到达位置1 */
}


static void auto_1_2(void)
{
  /* 往前开一点，防止当前十字路口误判 */
  track_drive(speed_medium);
  delay_ms(500);

  /* 前进控制 */
  while (1)
  {
    delay_ms(20);
    track_drive(speed_medium);

    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_cross())
    {
      track_stop();
      break;
    }
  }

  status.location = '2';    /* 到达位置2 */
}

/* 3为t字路口 */
static void auto_2_3(void)
{
  /* 往前开一点，防止当前十字路口误判 */
  track_drive(speed_medium);
  delay_ms(500);

  /* 前进控制 */
  while (1)
  {
    delay_ms(20);
    track_drive(speed_medium);

    /* 遇到T字路口，停车，跳出控制循环 */
    if (gray_is_t())
    {
      track_stop();
      break;
    }
  }
  status.location = '3';    /* 到达位置3 */
  track_turn_left(speed_medium);        /* T字路口左转，准备前往4号位置 */
}

static void auto_3_4(void)
{
  /* 往前开一点，防止当前十字路口误判 */
  track_drive(speed_medium);
  delay_ms(500);

  /* 前进控制 */
  while (1)
  {
    delay_ms(20);
    track_drive(speed_low);

    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_cross())
    {
      track_stop();
      break;
    }
  }

  status.location = '4';    /* 到达位置4 */
}

static void auto_4_5(void)
{
  /* 往前开一点，防止当前十字路口误判 */
  track_drive(speed_medium);
  delay_ms(500);

  /* 前进控制 */
  while (1)
  {
    delay_ms(20);
    track_drive(speed_medium);

    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_cross())
    {
      track_stop();
      break;
    }
  }

  status.location = '5';    /* 到达位置5 */
}


static void auto_5_6(void)
{
  /* 往前开一点，防止当前十字路口误判 */
  track_drive(speed_medium);
  delay_ms(500);

  /* 前进控制 */
  while (1)
  {
    delay_ms(20);
    track_drive(speed_medium);

    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_cross())
    {
      track_stop();
      break;
    }
  }

  status.location = '6';    /* 到达位置6 */
}


static void auto_6_7(void)
{
  /* 往前开一点，防止当前十字路口误判 */
  track_drive(speed_medium);
  delay_ms(500);

  /* 前进控制 */
  while (1)
  {
    delay_ms(20);
    track_drive(speed_medium);

    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_cross())
    {
      track_stop();
      break;
    }
  }
  status.location = '7';    /* 到达位置7 */
}

static void auto_7_8(void)
{
  /* 往前开一点，防止当前十字路口误判 */
  track_drive(speed_medium);
  delay_ms(500);

  /* 前进控制 */
  while (1)
  {
    delay_ms(20);
    track_drive(speed_medium);

    /* 遇到终点框的T字路口，不停车，跳出控制循环 */
    if (gray_is_t())
    {
      break;
    }
  }

  /* 再往前开1s再停车  */
  delay_ms(1000);
  track_stop();
  status.location = '8';    /* 到达位置8 */
}


static void auto_7_6(void)
{
  /* 往后开一点，防止当前十字路口误判 */
  track_reverse(speed_medium);
  delay_ms(500);

  /* 倒车循迹控制 */
  while (1)
  {
    delay_ms(20);
    track_reverse(speed_medium);

    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_cross())
    {
      track_stop();
      break;
    }
  }

  status.location = '6';      /* 到达位置6 */
}


static void auto_6_5(void)
{
  /* 往后开一点，防止当前十字路口误判 */
  track_reverse(speed_medium);
  delay_ms(500);

  /* 倒车循迹控制 */
  while (1)
  {
    delay_ms(20);
    track_reverse(speed_medium);

    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_cross())
    {
      track_stop();
      break;
    }
  }

  status.location = '5';      /* 到达位置5 */
}


static void auto_5_4(void)
{
  /* 往后开一点，防止当前十字路口误判 */
  track_reverse(speed_medium);
  delay_ms(500);

  /* 倒车循迹控制 */
  while (1)
  {
    delay_ms(20);
    track_reverse(speed_medium);

    /* 遇到十字路口，停车，跳出控制循环 */
    if (gray_is_cross())
    {
      track_stop();
      break;
    }
  }

  status.location = '4';      /* 到达位置4 */
}

/* 前进控制函数表 */
static void auto_drive_fun_lst(unsigned char destination)
{
  switch (destination)
  {
    case '1' :
      auto_0_1();
      break;
    case '2' :
      auto_1_2();
      break;
    case '3' :
      auto_2_3();
      break;
    case '4' :
      auto_3_4();
      break;
    case '5' :
      auto_4_5();
      break;
    case '6' :
      auto_5_6();
      break;
    case '7' :
      auto_6_7();
      break;
    case '8' :
      auto_7_8();
      break;
    default  :
      break;
  }
}

/* 后退控制函数表 */
static void auto_reverse_fun_lst(unsigned char destination)
{
  switch (destination)
  {
    case '6' :
      auto_7_6();
      break;
    case '5' :
      auto_6_5();
      break;
    case '4' :
      auto_5_4();
      break;
    default  :
      break;
  }
}


void auto_run(unsigned char location, unsigned char destination)
{

  if (status.destination == '0')  /* 发'0'，重新开始，调试用 */
  {
    status.location = '0';
    status.control_mode = automatic;
    status.destination = '0';
    return;
  }

  if (destination > location)   /* 正向运行 */
  {
    for (int i = location + 1; i <= destination; i++)
    {
      auto_drive_fun_lst(i);
    }
  }

  else if (destination < location)   /* 反向运行 */
  {
    for (int i = location - 1; i >= destination; i--)
    {
      auto_reverse_fun_lst(i);
    }
  }
}


/* 前进一步测试 */
void auto_ctrl_test(void)
{
  while (1)
  {
    hostpc_cmd_refresh();   /* 更新运行模式与目的地 */

    if (status.location == status.destination - 1)
    {
      switch (status.destination)
      {
        case '0' :
          status.destination = 0;
          break;
        case '1' :
          auto_0_1();
          break;
        case '2' :
          auto_1_2();
          break;
        case '3' :
          auto_2_3();
          break;
        case '4' :
          auto_3_4();
          break;
        case '5' :
          auto_4_5();
          break;
        case '6' :
          auto_5_6();
          break;
        case '7' :
          auto_6_7();
          break;
        case '8' :
          auto_7_8();
          break;
        default  :
          break;
      }
      hostpc_send(0x01);          /* 通知上位机 */
    }
  }
}


